#include "node.h"
#include <osgDB/ReadFile>
#include <iostream>
#include <osg/Vec3d>
#include <osg/StateSet>
#include <osg/BlendColor>
#include <osg/BlendFunc>

osg::ref_ptr<osgManipulator::CommandManager> cmdMgr = new osgManipulator::CommandManager();
int node_counter = 0;

//----------------------------------------------------------------------
Node::Node(void)
//----------------------------------------------------------------------
{
  _id = node_counter++;
  _parent = 0;
  _node = new osg::Node();
  _group = new osg::Group();
  _transform = new osgManipulator::Selection();
  _group->addChild(_transform.get());
  _transform->addChild(_node.get());
  _opacity = 1;

  _shown = true;
}
//----------------------------------------------------------------------
Node::Node(Node* parent)
//----------------------------------------------------------------------
{
  _id = node_counter++;
  _parent = 0;
  _node = new osg::Node();
  _group = new osg::Group();
  _transform = new osgManipulator::Selection();
  _group->addChild(_transform.get());
  _transform->addChild(_node.get());
  _opacity = 1;

  _shown = true;

  SetParent(parent);
}
//----------------------------------------------------------------------
Node::~Node(void)
//----------------------------------------------------------------------
{
}
//----------------------------------------------------------------------
void Node::SetParent( Node *n )
//----------------------------------------------------------------------
{
  if(n == _parent) return;
  if(_parent) _parent->RemoveChild(this);
  _parent = n;
  if(_parent) _parent->AddChild2(this);
}
//----------------------------------------------------------------------
bool Node::AddChild(Node *n)
//----------------------------------------------------------------------
{
  //TODO - check that n is not an anchestor of mine
  if(!n) return false;
  if(n->GetParent() == this ) return false;
  n->SetParent(this);
  return true;
}
//----------------------------------------------------------------------
void Node::AddChild2(Node *n)
//----------------------------------------------------------------------
{
  //n->_parent already set to me
  _children.push_back(n);
  _transform->addChild( n->Get() );
}
//----------------------------------------------------------------------
bool Node::RemoveChild( Node *n)
//----------------------------------------------------------------------
{
  std::vector<Node*>::iterator it;
  for( it=_children.begin(); it<_children.end(); it++ ) 
  {
    if( *it == n )
    {
      _transform->removeChild(n->Get());
      _children.erase(it);
      return true;
    }
  }              
  return false;
}
//----------------------------------------------------------------------
bool Node::Load(std::string filename)
//----------------------------------------------------------------------
{
  osg::Node* node = osgDB::readNodeFile(filename);
  if (!node) 
  {
    std::cout << "DemoEngine: No data loaded" << std::endl;
    return false;
  }
  if(_shown) _transform->removeChild(_node.get());
  _node = node;
  if(_shown) _transform->addChild(_node.get());
  _filename = filename;
  return true;  
}
//----------------------------------------------------------------------
void Node::Show(bool show)
//----------------------------------------------------------------------
{
  if( show && !_shown )
  {
    _group->addChild(_transform.get());
    _shown = true;
  }
  if( !show && _shown )
  {
    _group->removeChild(_transform.get());
    _shown = false;
  }
}
//----------------------------------------------------------------------
void Node::ShowTool(int tool)
//----------------------------------------------------------------------
{
 if(_dragger.get())
 {
   cmdMgr->disconnect(*_dragger);
   _group->removeChild(_dragger.get());
   _dragger = 0; // dovrei cancellarlo
 }

 _dragger = createDragger(tool);
 if(_dragger.get())
 {
    const osg::BoundingSphere& bs= _group->getBound();
    float scale = bs.radius() * 1.1;
    osg::Matrix m = osg::Matrix::scale(scale, scale, scale)*osg::Matrix::translate(bs.center());

    _group->addChild(_dragger.get());
    _dragger->setMatrix(m);
    cmdMgr->connect(*_dragger, *_transform);
 }
}
//----------------------------------------------------------------------
void Node::Decompose()
//----------------------------------------------------------------------
{
  _transform->getMatrix().decompose(_trans,_rot,_scale,_scale_org);
}
//----------------------------------------------------------------------
void Node::Ricompose()
//----------------------------------------------------------------------
{
  _transform->setMatrix( //osg::Matrix::translate(-_scale_org)*
                         osg::Matrix::rotate(_rot) * 
                         osg::Matrix::scale(_scale) * 
                         osg::Matrix::translate(_trans) );

          /* --- e il _referenceFrame ?? - per adesso me ne frego
          bool PositionAttitudeTransform::computeLocalToWorldMatrix(Matrix& matrix,NodeVisitor*) const
          {
              if (_referenceFrame==RELATIVE_RF)
              {
                  matrix.preMult(osg::Matrix::translate(-_pivotPoint)*
                                osg::Matrix::scale(_scale)*
                                osg::Matrix::rotate(_attitude)*
                                osg::Matrix::translate(_position));
              }
              else // absolute
              {
                  matrix = osg::Matrix::translate(-_pivotPoint)*
                          osg::Matrix::scale(_scale)*
                          osg::Matrix::rotate(_attitude)*
                          osg::Matrix::translate(_position);
              }
              return true;
          }
          */
}
//----------------------------------------------------------------------
void Node::SetPosition( Vec pos )
//----------------------------------------------------------------------
{
  Decompose();
  _trans = pos.Get();
  Ricompose();
}
//----------------------------------------------------------------------
void Node::SetRotation( Quat rot )
//----------------------------------------------------------------------
{
  Decompose();
  _rot = rot.Get();
  Ricompose();
}
//----------------------------------------------------------------------
void Node::SetScale( Vec scale )
//----------------------------------------------------------------------
{
  Decompose();
  _scale = scale.Get();
  Ricompose();
}
//----------------------------------------------------------------------
void Node::Move(  Vec p )
//----------------------------------------------------------------------
{
  Decompose();
  _trans += p.Get();
  Ricompose();
}
//----------------------------------------------------------------------
void Node::Scale( Vec s )
//----------------------------------------------------------------------
{
  Decompose();
  double x = _scale.x() * s.x;
  double y = _scale.y() * s.y;
  double z = _scale.z() * s.z;
  _scale = osg::Vec3d(x,y,z);
  Ricompose();
}
//----------------------------------------------------------------------
void Node::Scale( float s )
//----------------------------------------------------------------------
{
  Decompose();
  _scale *= s;
  Ricompose();
}
//----------------------------------------------------------------------
void Node::Rotate(Quat q)
//----------------------------------------------------------------------
{
  Decompose();
  _rot *= q.Get();
  Ricompose();
}
//----------------------------------------------------------------------
void Node::RotateX(float a)
//----------------------------------------------------------------------
{
  Decompose();
  _rot *= Quat(a,Vec(1,0,0)).Get();
  Ricompose();
}
//----------------------------------------------------------------------
void Node::RotateY(float a)
//----------------------------------------------------------------------
{
  Decompose();
  _rot *= Quat(a,Vec(0,1,0)).Get();
  Ricompose();
}
//----------------------------------------------------------------------
void Node::RotateZ(float a)
//----------------------------------------------------------------------
{
  Decompose();
  _rot *= Quat(a,Vec(0,0,1)).Get();
  Ricompose();
}
//----------------------------------------------------------------------
std::string Node::Repr()
//----------------------------------------------------------------------
{
  std::ostringstream p; 
  if (_parent) p << "'node" << _parent->_id << "'"; else p << "none";

  std::ostringstream os; 
  os << "id      = " << "'node" << _id << "'" << std::endl;
  os << "parent  = " << p.str() << std::endl;
  os << "name    = " <<  "'" << Name() << "'" << std::endl;
  os << "file    = " << "'" << _filename << "'" << std::endl;
  os << "children= " << GetNumberOfChildren() << std::endl;
  os << "visible = " << IsShown() << std::endl;
  os << "pos     = " << GetPosition().Repr() << std::endl;
  os << "rot     = " << GetRotation().Repr() << std::endl;
  os << "scale   = " << GetScale().Repr() << std::endl;
  return os.str();
}
//----------------------------------------------------------------------
std::string Node::Name()
//----------------------------------------------------------------------
{
  return _node->getName();
}
//----------------------------------------------------------------------
void Node::SetOpacity( double opacity )
//----------------------------------------------------------------------
{
  osg::StateSet *ss = _node->getOrCreateStateSet();

  _opacity = opacity;

  if(opacity == 1)
  {
    ss->setMode(GL_BLEND,osg::StateAttribute::OFF);
  }
  else
  {
    ss->setMode(GL_BLEND,osg::StateAttribute::ON);

    osg::BlendColor *bc = NULL;
    bc = static_cast< osg::BlendColor * >( ss->getAttribute( osg::StateAttribute::BLENDCOLOR ) );
    if(!bc)
    {
      bc = new osg::BlendColor();
      ss->setAttributeAndModes(bc, osg::StateAttribute::ON);
    }
    bc->setConstantColor(osg::Vec4( opacity, opacity, opacity, opacity ));

    osg::BlendFunc *bf = NULL;
    bf = static_cast< osg::BlendFunc * >( ss->getAttribute( osg::StateAttribute::BLENDFUNC ) );
    if(!bf) 
    {
      bf = new osg::BlendFunc(osg::BlendFunc::CONSTANT_COLOR, osg::BlendFunc::ONE_MINUS_CONSTANT_COLOR);
      ss->setAttributeAndModes(bf, osg::StateAttribute::ON);
    }
  }
}

